#include "ms575_algbacks3s4ldmeasure.h"
using namespace MeasurE575;

AlgBackS3S4LDmeasure::AlgBackS3S4LDmeasure(int i):AlgBase(i)
{

}

void AlgBackS3S4LDmeasure::operator()(cv::Mat &srcImg,
                                      int id,
                                      std::map<string, float> &measRes,
                                      int fixtureId)
{
//back  S3-> 27  //back  S4-> 28
    //int LDmeasure(cv::Mat srcimg, const vector<int> paras, vector<cv::Point2f>& respts);

    std::vector<int> paras5,paras6;
    for(int j=0;j< m_dataAllVec[27].m_edgeParam.cols;j++)
    {
        paras5.push_back(m_dataAllVec[27].m_edgeParam.at<int>(0,j));
    }
    for(int j=0;j< m_dataAllVec[28].m_edgeParam.cols;j++)
    {
        paras6.push_back(m_dataAllVec[28].m_edgeParam.at<int>(1,j));
    }
    std::vector<cv::Point2f> resultS3,resultS4;
    std::vector<cv::Rect> rangS3Vec,rangS4Vec;

    {
        TemplateMatch(srcImg,m_dataAllVec[27],rangS3Vec);
        edgedetect obj;
        obj.LDmeasure(srcImg(rangS3Vec[0]), paras5, resultS3);
        for(auto &ci:resultS3)
        {
            ci.x+=rangS3Vec[0].x;
            ci.y+=rangS3Vec[0].y;
        }

    }

    {
        TemplateMatch(srcImg,m_dataAllVec[28],rangS4Vec);
        edgedetect obj;
        obj.LDmeasure(srcImg(rangS4Vec[0]), paras6, resultS4);
        for(auto &ci:resultS4)
        {
            ci.x+=rangS4Vec[0].x;
            ci.y+=rangS4Vec[0].y;
        }
    }
    std::unique_lock<std::mutex> lock(*AlgBase::mtxptrVec[id]);
    while (!AlgBase::flagVec[id])
    {
        (*AlgBase::cvarptrVec[id]).wait(lock);
    }
#pragma omp parallel sections
    {
        #pragma omp section
        {

            for(auto &pt2:resultS3)
            {
                cv::Point2f ptsOut;
                CoordinateTransformationRelateToDot(datumBackVec[id], pt2,ptsOut);
                pt2=ptsOut;
            }

         }
        #pragma omp section
          {

            for(auto &pt2:resultS4)
            {
                cv::Point2f ptsOut;
                CoordinateTransformationRelateToDot(datumBackVec[id], pt2,ptsOut);
                pt2=ptsOut;
            }
          }
     }

    std::string prefix = m_xmlFileNamePre + to_string(fixtureId) + "_";
    measRes.insert(std::make_pair(string("FAI183-LD-Y-distance_pixel"), fabs(resultS3[3].y-resultS4[0].y)));
    float resultS31real = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI183-LD-Y-distance")], fabs(resultS3[3].y-resultS4[0].y));
    measRes.insert(std::make_pair(string("FAI183-LD-Y-distance_real"), resultS31real));
    measRes.insert(std::make_pair(string("FAI185-LD-Y-distance_pixel"), fabs(resultS3[3].y-resultS3[5].y)));
    float resultS32real = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI185-LD-Y-distance")], fabs(resultS3[3].y-resultS3[5].y));
    measRes.insert(std::make_pair(string("FAI185-LD-Y-distance_real"), resultS32real));
    measRes.insert(std::make_pair(string("FAI186-LD-X-distance_pixel"), fabs(resultS3[0].x-resultS3[4].x)));
    float resultS33real = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI186-LD-X-distance")], fabs(resultS3[0].x-resultS3[4].x));
    measRes.insert(std::make_pair(string("FAI186-LD-X-distance_real"), resultS33real));
    measRes.insert(std::make_pair(string("FAI187-LD-X-distance_pixel"), fabs(resultS3[0].x-resultS3[2].x)));
    float resultS34real = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI187-LD-X-distance")], fabs(resultS3[0].x-resultS3[2].x));
    measRes.insert(std::make_pair(string("FAI187-LD-X-distance_real"), resultS34real));
    measRes.insert(std::make_pair(string("FAI188-LD-X-distance_pixel"), fabs(resultS3[0].x+resultS3[6].x)));
    float resultS35real = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI188-LD-X-distance")], fabs(resultS3[0].x+resultS3[6].x));
    measRes.insert(std::make_pair(string("FAI188-LD-X-distance_real"), resultS35real));

    measRes.insert(std::make_pair(string("FAI189-LD-X-distance_pixel"), fabs(resultS3[1].x-resultS3[4].x)));
    float resultS36real = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI189-LD-X-distance")], fabs(resultS3[1].x-resultS3[4].x));
    measRes.insert(std::make_pair(string("FAI189-LD-X-distance_real"), resultS36real));
}
